//#include<36064s.h>
#include<36064.h>
#include <string.h>
#include "vs-wrc003.h"
//#include <machine.h>
//関数

#pragma interrupt(intsci3)
#pragma interrupt(Int_Pwm)
#pragma interrupt(Int_Sv)

//GAME_PAD
#define PS_SEL_raise	IO.PDR2.BIT.B3 = 1
#define PS_SEL_fall		IO.PDR2.BIT.B3 = 0
#define PS_CLK_raise	IO.PDR2.BIT.B4 = 1
#define PS_CLK_fall		IO.PDR2.BIT.B4 = 0
#define PS_CMD_raise	IO.PDR2.BIT.B0 = 1
#define PS_CMD_fall		IO.PDR2.BIT.B0 = 0
#define PS_DAT			IO.PDR7.BIT.B5
#define PS_CLK_BIT		IO.PDR2.BIT.B4

static const BYTE CMD_config_mode_enter[] =     {0x01,0x43,0x00,0x01, 0x00,0x00,0x00,0x00, 0x00};
static const BYTE CMD_config_mode_exit[] =      {0x01,0x43,0x00,0x00, 0x00,0x00,0x00,0x00, 0x00};
static const BYTE CMD_config_mode_exit2[] =      {0x01,0x43,0x00,0x00, 0x5a,0x5a,0x5a,0x5a, 0x5a};
static const BYTE CMD_set_mode_and_lock[] =     {0x01,0x44,0x00,0x01, 0x03,0x00,0x00,0x00, 0x00};
static const BYTE CMD_query_model_and_mode[] =  {0x01,0x45,0x00,0x5a, 0x5a,0x5a,0x5a,0x5a, 0x5a};
static const BYTE CMD_vibration_enable[] =      {0x01,0x4d,0x00,0x00, 0x01,0xff,0xff,0xff, 0xff};
static const BYTE CMD_vibration_disnable[] =    {0x01,0x4d,0x00,0xff, 0xff,0xff,0xff,0xff, 0xff};
static const BYTE CMD_query_DS2_analog_mode[] = {0x01,0x41,0x00,0x5a, 0x5a,0x5a,0x5a,0x5a, 0x5a};
static const BYTE CMD_set_DS2_native_mode[] =   {0x01,0x4f,0x00,0xff, 0xff,0x03,0x00,0x00, 0x00};
static const BYTE CMD_read_data[] =             {0x01,0x42,0x00,0x00, 0x00,0x00,0x00,0x00, 0x00}; 
static const BYTE CMD_read_data2[] =            {0x01,0x42,0x00,0x00, 0x00,0x00,0x00,0x00, 0x00,0x00,0x00,0x00, 
											0x00,0x00,0x00,0x00, 0x00,0x00,0x00,0x00, 0x00};

int PS_Set_AN();
BYTE Ds2rw(const BYTE *cmd);
void Ds2Com(const BYTE * cmd,BYTE txLen);
void Sv_Run(int deg0,int deg1,int deg2,int deg3);

//MtrPWM ********************************************************************//
void InitTZ();
void InitAD();

UINT MTR_PWM_CYC = 36864;
BYTE MTR[4] = {0x80,0x80,0x80,0x80};
BYTE MTR_CW_GAIN[4] = {255,255,255,255};
BYTE MTR_CCW_GAIN[4] = {255,255,255,255};
BYTE MTR_EN = 0x00;
BYTE SV_EN = 0x0F;


static unsigned char Mtr_Pwm_F;
unsigned int cycle,duty[4],temp_TZ0cycle;
unsigned int sv_cnt = 0;
unsigned int sv_cnt2 = 0;
unsigned int sv_output[4] = {0,0,0,0};
int SV_DEG[4] = {0,0,0,0};
void Sv_Setoutputvalue();
int SV_ToDEG[4] = {0,0,0,0};
unsigned int SV_Step = 0;


void WRC004_MtrEn(BYTE value){
	if(value == 0){
		MTR_EN = 0;
	}
	else{
		MTR_EN = 1;
	}
}

void InitTZ()
{
	TZ.TSTR.BIT.STR0=0;
	TZ.TSTR.BIT.STR1=0;
	TZ.TMDR.BYTE =	0x00;	//通常
	TZ.TFCR.BYTE =	0x00;	//通常
	TZ.TPMR.BYTE =	0x00;	//FTIOC1,FTIOD1がPWM動作
	
	TZ.TOER.BYTE =	0xF0;	//TZ0：出力許可
	TZ.TOCR.BYTE =	0x30;	//TZ0：はじめ1出力	,1:1出力
	
	IO.PCR6 = 0xFF;
	IO.PDR6.BYTE = 0x00;
}

void InitMtrPwm()
{
	
	UINT tempSint = 0, i = 0;
	//BYTE temp = 0;
	UDWORD temp2 = 0;
	
	TZ0.TCR.BYTE =	0x01;	//clr禁止、立上りエッジ、φでカウント

	TZ0.TIORA.BYTE=	0xAA;		//コンペアマッチで1出力
	TZ0.TIORC.BYTE=	0xAA;
	TZ0.TSR.BYTE =	0x00;	//エラーフラグクリア
	TZ0.TIER.BYTE =	0x0F;	//コンペアで割り込み

	TZ0.TCNT=0;
	
		cycle=MTR_PWM_CYC;
		
		
		for(i=0;i<4;i++){
			if(MTR[i]==0 || MTR[i]==0x80)
				tempSint=0;
			else if(MTR[i]<0x80)
				tempSint=MTR[i]*MTR_CW_GAIN[i];
			else if(MTR[i]>0x80)
				tempSint=(~MTR[i]+1)*MTR_CCW_GAIN[i];
			
			temp2=(UDWORD)tempSint*(UDWORD)cycle;
			duty[i]=(UINT)(temp2/0x7FFF);
		}
		
		TZ0.GRA=cycle;
		TZ0.GRB=cycle;
		TZ0.GRC=cycle;
		TZ0.GRD=cycle;
		temp_TZ0cycle=cycle;
		Mtr_Pwm_F=0;

}

BYTE Int_Pwm()
{
	static BYTE temp_TZ0STS;
	static UINT temp[4];
	
    {
        int i = 0;
        for (i = 0; i < 4; i++) {
            temp[i] = 0;
        }
    }

	if(Mtr_Pwm_F==1)
	{
		temp_TZ0STS += (TZ0.TSR.BYTE & 0x0F);
		
		if(temp_TZ0STS == 0x0F)
		{
			TZ.TSTR.BIT.STR0=0;
			TZ0.TSR.BYTE = 0x00;
			temp_TZ0cycle+=cycle;
			TZ0.GRA=temp_TZ0cycle;
			TZ0.GRB=temp_TZ0cycle;
			TZ0.GRC=temp_TZ0cycle;
			TZ0.GRD=temp_TZ0cycle;
			
			TZ0.TIORA.BYTE=	0xAA;		//コンペアマッチで1出力
			
			if(MTR_EN == 0){
				if(sv_cnt2>7){
					TZ0.TIORC.BYTE=	0xAA;
					TZ.TOCR.BIT.TOD0 = (~SV_EN>>3 & 0x01);
					TZ.TOCR.BIT.TOC0 = (~SV_EN>>2 & 0x01);
					TZ.TOER.BIT.EC0 = (~SV_EN>>3  & 0x01);
					TZ.TOER.BIT.ED0 = (~SV_EN>>2  & 0x01);
				}
				else{
					TZ0.TIORC.BYTE=	0x11;
					TZ.TOCR.BIT.TOD0 = 0;
					TZ.TOCR.BIT.TOC0 = 0;
					TZ.TOER.BIT.EC0 = (SV_EN>>3  & 0x01);
					TZ.TOER.BIT.ED0 = (SV_EN>>2  & 0x01);	
				}
			}
			else{
				TZ0.TIORC.BYTE=	0xAA;
				TZ.TOCR.BIT.TOD0 = 0;
				TZ.TOCR.BIT.TOC0 = 0;
			}
			if(sv_cnt2>7)
					sv_cnt2 = 0;
			if(sv_cnt2 == 1){
				TZ1.TIER.BYTE =	0x0D;
				TZ.TOER.BIT.EC1 = 0;
				TZ.TOER.BIT.ED1	= 0;
				TZ.TSTR.BIT.STR1=1;		//カウントスタート		
			}
			else{
				TZ1.TIER.BYTE =	0x00;
			}
			Mtr_Pwm_F=0;
			temp_TZ0STS=0;
			TZ.TSTR.BIT.STR0=1;
		}
	}
	
	else if(Mtr_Pwm_F==0 && (TZ0.TSR.BYTE & 0x0F) != 0)
	{
		TZ.TSTR.BIT.STR0=0;
		TZ0.TSR.BYTE = 0x00;
		if(MTR_EN == 0){
			duty[2] = sv_output[2]/2;
			duty[3] = sv_output[3]/2;
		}
		TZ0.GRA+=duty[0];
		TZ0.GRB+=duty[1];
		TZ0.GRC+=duty[2];
		TZ0.GRD+=duty[3];
		
		TZ0.TIORA.BYTE=	0x99;		//コンペアマッチで0出力
		TZ0.TIORC.BYTE=	0x99;
		Mtr_Pwm_F=1;
		temp_TZ0STS=0;
		TZ.TSTR.BIT.STR0=1;
		sv_cnt2++;
	}
	
	TZ0.TSR.BYTE = 0x00;
	return 0;
}

void Mtr_setCWGain(BYTE mt1,BYTE mt2,BYTE mt3,BYTE mt4){
	MTR_CW_GAIN[0] = mt1;
	MTR_CW_GAIN[1] = mt2;
	MTR_CW_GAIN[2] = mt3;
	MTR_CW_GAIN[3] = mt4;
}
void Mtr_setCCWGain(BYTE mt1,BYTE mt2,BYTE mt3,BYTE mt4){
	MTR_CCW_GAIN[0] = mt1;
	MTR_CCW_GAIN[1] = mt2;
	MTR_CCW_GAIN[2] = mt3;
	MTR_CCW_GAIN[3] = mt4;
}
void Mtr_Run(BYTE mt1,BYTE mt2,BYTE mt3,BYTE mt4)
{
	//BYTE i,j,out3,out5;
	BYTE i,out3,out5;
	UDWORD tempSint = 0;
	UDWORD temp2;
	static UINT beforeMtrduty[4];
	BYTE mask;
	
	out3=0;
	out5=0;
	MTR[0] = mt1;
	MTR[1] = mt2;
	MTR[2] = mt3;
	MTR[3] = mt4;
		//周期のセット
		cycle=MTR_PWM_CYC;
		if(cycle<0x1000)
			cycle=0x1000;
		

		//デューティのセット
		for(i=0;i<4;i++){
			if(MTR[i]==0 || MTR[i]==0x80)
				tempSint=0;
				
			else if(MTR[i]<0x80)
				tempSint=(UDWORD)MTR[i]*MTR_CW_GAIN[i];
			
			else if(MTR[i]>0x80)
				tempSint=(UDWORD)(0x100-MTR[i])*MTR_CCW_GAIN[i];
							
			beforeMtrduty[i]=tempSint;
			
			temp2=tempSint*(UDWORD)cycle;
			duty[i]=(UINT)(temp2/0x7FFF);
			
			if(duty[i]<0x200)
				duty[i]=0x200;
			else if(cycle-duty[i]<0x300)
				duty[i]=cycle-0x300;
		}
		
		//TZが止まっていたらスタート
		if(TZ.TSTR.BIT.STR0!=1){
			Mtr_Pwm_F=0;
			TZ0.TCNT=0;
			temp_TZ0cycle=cycle;
			TZ0.GRA=cycle;
			TZ0.GRB=cycle;
			TZ0.GRC=cycle;
			TZ0.GRD=cycle;
			TZ0.TIORA.BYTE=	0xAA;		//コンペアマッチで1出力
			TZ0.TIORC.BYTE=	0xAA;
			TZ.TSTR.BIT.STR0=1;		//カウントスタート
		}
		// LL :HiZ（out：ZZ）  HL,LH:drive  HH:ブレーキ（out：LL）に変更必要 

		for(i=0;i<4;i++)
		{
			mask=~(0x01<<i);
			//ブレーキ	
			if(MTR[i]==0x80){
				if(i<2)
					out3 |= 0x03<<(i*2);
				else
					out5 |= 0x03<<((i-1)*2);
				duty[i]=0x200;
				TZ.TOER.BYTE = TZ.TOER.BYTE | ~mask;
				
			}
			//フリー
			else if(MTR[i]==0){
				if(i<2)
					out3 |= 0x00<<(i*2);
				else
					out5 |= 0x00<<((i-1)*2);
				
				duty[i]=0x200;
				TZ.TOER.BYTE = TZ.TOER.BYTE | ~mask;
			}
			else if(MTR[i]<0x80){
				TZ.TOER.BYTE = TZ.TOER.BYTE & mask;
				if(i<2)
					out3 |= 0x01<<(i*2);
				else
					out5 |= 0x01<<((i-1)*2);				
			}
			else if(MTR[i]>0x80){
				TZ.TOER.BYTE = TZ.TOER.BYTE & mask;
				if(i<2)
					out3 |= 0x02<<(i*2);
				else
					out5 |= 0x02<<((i-1)*2);				
			}
		}

	IO.PDR3.BYTE=out3;
	if(MTR_EN == 0){
		IO.PDR5.BYTE = 0;
	}
	else{
		IO.PDR5.BYTE=out5;
		if((out5 & 0x20) != 0)
			IO.PDR7.BIT.B4=1;
		else
			IO.PDR7.BIT.B4=0;
	}
}


void InitSv(){

	TZ1.TCR.BYTE =	0x00;	//clr禁止、立上りエッジ、φ/4でカウント
	TZ1.TSR.BYTE =	0x00;	//エラーフラグクリア
	TZ1.TIER.BYTE =	0x0D;
	TZ.TOCR.BIT.TOD1 = 1;
	TZ.TOCR.BIT.TOC1 = 1;
	TZ1.TCNT=0;
	TZ1.TIORA.BYTE = 0x00;
	TZ1.TIORC.BYTE = 0x11;
	TZ1.GRA = 36864;
	Sv_Setoutputvalue();
	Sv_Run(0,0,0,0);
	TZ.TOER.BIT.EC1=1;
	TZ.TOER.BIT.ED1=1;
}

void Sv_Setoutputvalue()
{
	int i;
	for(i=0;i<4;i++){
		if(SV_Step>0){
			SV_DEG[i] = SV_DEG[i]+((double)(SV_ToDEG[i]-SV_DEG[i])/SV_Step);
			SV_Step--;
		}
		else if(SV_Step == 0){
			SV_DEG[i] = SV_ToDEG[i];
		}
		if(SV_DEG[i]<14745 && SV_DEG[i] > -14745)
			sv_output[i] = (unsigned int)((long)22118 + SV_DEG[i]);
		else if(SV_DEG[i] >= 14745)
			sv_output[i] = (unsigned int)((long)22118 + 14745);
		else if(SV_DEG[i] <= -14745)
			sv_output[i] = (unsigned int)((long)22118 - 14745);
	}
}

void Sv_PawerOn(BYTE value){
	SV_EN = value&0x0F;
}

void Sv_Run(int deg0,int deg1,int deg2,int deg3)
{
	SV_DEG[0] = deg0;
	SV_DEG[1] = deg1;
	SV_DEG[2] = deg2;
	SV_DEG[3] = deg3;
	Sv_Setoutputvalue();

	if(TZ.TSTR.BIT.STR1==0){
		TZ1.TCNT=0;
		TZ.TOER.BIT.EC1 = 0;
		TZ.TOER.BIT.ED1	= 0;
		TZ.TSTR.BIT.STR1=1;		//カウントスタート
		sv_cnt = 0;
	}
}

void Sv_Move(int deg0,int deg1,int deg2,int deg3,unsigned int Step)
{
	SV_ToDEG[0] = deg0;
	SV_ToDEG[1] = deg1;
	SV_ToDEG[2] = deg2;
	SV_ToDEG[3] = deg3;	
	SV_Step = Step;
}

void Sv_Move_Wait(int deg0,int deg1,int deg2,int deg3,unsigned int Step)
{
	SV_ToDEG[0] = deg0;
	SV_ToDEG[1] = deg1;
	SV_ToDEG[2] = deg2;
	SV_ToDEG[3] = deg3;	
	SV_Step = Step;
	while(SV_Step != 0);
}


unsigned char Int_Sv()
{
	if(TZ1.TSR.BIT.IMFC){
		TZ.TOER.BIT.EC1 = 1;
	}
	if(TZ1.TSR.BIT.IMFD){
		TZ.TOER.BIT.ED1 = 1;
	}
	
	if(TZ1.TSR.BIT.IMFA){
			Sv_Setoutputvalue();
			TZ.TSTR.BIT.STR1=0;		//カウントストップ
			TZ.TOCR.BIT.TOC1 = (SV_EN  & 0x01);
			TZ.TOCR.BIT.TOD1 = (SV_EN>>1 & 0x01);
			TZ.TOER.BIT.EC1 = 1;
			TZ.TOER.BIT.ED1 = 1;
			TZ1.GRC=sv_output[1];
			TZ1.GRD=sv_output[0];
			TZ1.TCNT=0;
	}


	TZ1.TSR.BYTE = 0x00;
	return 0;
}


//ADin ********************************************************************//
void InitAD()
{
	AD.ADCR.BYTE=0x00;	
	AD.ADCSR.BYTE=0x03;
	AD.ADCSR.BIT.ADST=1;
}

UINT AdRead(BYTE ch)
{
	//BYTE temp;
	UDWORD x;
	
	AD.ADCSR.BIT.ADST=0;
	AD.ADCR.BYTE=0x00;
	AD.ADCSR.BYTE=0;
	if(ch<0x06)
		AD.ADCSR.BYTE=ch;
		
	AD.ADCSR.BIT.ADST=1;
	
	while (AD.ADCSR.BIT.ADF!=1);
	AD.ADCSR.BIT.ADF=0;	
	AD.ADCSR.BIT.ADST=0;
	
	
	switch(ch%4){
		case 0:
			x=AD.ADDRA;
			break;
		case 1:
			x=AD.ADDRB;
			break;
		case 2:
			x=AD.ADDRC;
			break;
		case 3:
			x=AD.ADDRD;
			break;
		default:
			x = 0;
	}
	return (UINT)(x>>6);
}

//buzzer **********************************************************************//
BYTE BZR_PITCH;
BYTE BZR_VOL;

void BuzzerSet(BYTE pitch , BYTE vol){
	
	TV.TCRV1.BYTE=0x01;
	TV.TCRV0.BYTE=0x10;
	TV.TCSRV.BYTE=0x06;

	BZR_PITCH = pitch;
	BZR_VOL = vol;
	
	TV.TCRV0.BIT.CKS=0;
	TV.TCORA=(BYTE)((float)BZR_VOL*((float)BZR_PITCH/256.0));
	TV.TCORB=BZR_PITCH;
}

void BuzzerStart(){
	TV.TCRV0.BIT.CKS=3;
}

void BuzzerStop(){
	TV.TCRV0.BIT.CKS=0;
}



//SCI ************************************************************************//

volatile BYTE sci_rxrb[SCI_RB_SIZE];
volatile BYTE scicombuff[SCI_RB_SIZE];
BYTE rb_rd_num;
BYTE rb_wr_num;
BYTE scireadcnt;
const BYTE s[16]={"0123456789ABCDEF"};

BYTE intsci3(){
	//BYTE str[4],temp,next;
	BYTE temp,next;

	if((SCI3.SSR.BYTE & 0x18)==0 && SCI3.SSR.BIT.RDRF == 1)
	{          
		next=(rb_wr_num+1)%SCI_RB_SIZE;
		
		if(rb_rd_num!=next){
			SCI3.SSR.BYTE &= 0x87;
			sci_rxrb[rb_wr_num] = SCI3.RDR;			//  データを受け取りdataに保存 
			rb_wr_num=next;
			return 0;
		}
	}
	
	temp=SCI3.RDR;
	SCI3.SSR.BYTE &= 0x87;  
    return 1;
}


BYTE SciByteRx(BYTE *data)
{
	if(rb_rd_num!=rb_wr_num)	//受信バッファ空じゃなかったら
	{	
		*data=sci_rxrb[rb_rd_num];
		rb_rd_num=(rb_rd_num+1)%SCI_RB_SIZE;
		return 1;
	}
	else	
		return 0;
}


void InitSci3(BYTE baudrate,BYTE parity ,BYTE stop)
{
	UINT dmy;
	BYTE smr = 0;
	scireadcnt=0;
	
	switch(parity){
		case even:
			smr = 0x20;
		break;
		case odd:
			smr = 0x30;
		break;
		case non:
		default:
			smr = 0x00;
		break;
	}
	if(stop == 2){
		smr |= 0x08;
	}
	
	SCI3.SCR3.BYTE = 0;       //ｼﾘｱﾙｺﾝﾄﾛｰﾙﾚｼﾞｽﾀ(SCIの送信/受信動作設定)
	SCI3.SMR.BYTE = smr;       //ｼﾘｱﾙﾓｰﾄﾞﾚｼﾞｽﾀ(SCIの通信ﾌｫｰﾏｯﾄの設定)
	SCI3.BRR = (BYTE)baudrate;	          //1bps
	
	for(dmy = 280;dmy > 0;dmy--); //Wait 1bit transfer time
	SCI3.SCR3.BYTE = 0x70;    //Enable Tx & Rx ,enable Rx interrupt
	IO.PMR1.BIT.TXD=1;
	SCI3.SSR.BYTE &= 0x80;   //Clear receive error flag
}

void SciByteTx(BYTE data)
{
	
	SCI3.SCR3.BIT.TE=1;
	while(SCI3.SSR.BIT.TDRE!=1);

	SCI3.TDR=data;
	
	while(SCI3.SSR.BIT.TEND!=1);
	SCI3.SCR3.BIT.TE=0;
}


void SciStrTx(BYTE *str,UINT dataleng)
{
	UINT i;
	
	SCI3.SCR3.BIT.TE=1;
	
	for(i=0;i<dataleng;i++)
		SciByteTx(str[i]);
}

//GAME_PAD ********************************************************//
BYTE PS_DATA[10];
//受信データ
static BYTE PADRD[30];
BYTE Ds2rw(const BYTE *cmd){
	
	BYTE TempCmd;
	BYTE i,j,data,clk_cnt = 1;
	

	data=0;
	PS_CLK_raise;
	PS_CMD_raise;
	TempCmd = *cmd;

  
  for(j=0;j<5;j++);	//muda
  
  for(i=0;i<8;i++){
    if( (TempCmd & 0x01) == 0) /* CMDセット */
        PS_CMD_fall;
    else
        PS_CMD_raise;

    TempCmd = TempCmd >> 1;
    PS_CLK_fall;
    
    for(j=0;j<5;j++);	//muda
	
	if(PS_DAT!=0)
    	data+=clk_cnt;
	PS_CLK_raise;

    clk_cnt*=2;
    
  }
  PS_CMD_raise;
  return data;
}


//各コマンド送受信
void Ds2Com(const unsigned char * cmd,unsigned char txLen){
  unsigned char i,j;
  PS_SEL_fall;
  
  for(j=0;j<5;j++);	//muda
  
  for(i = 0; i < txLen;i++){
    for(j=0;j<30;j++);	//muda
	PADRD[i]=Ds2rw(cmd+i);
  }
  
  PS_SEL_raise;
  
}


int PS_Set_AN()
{
	int a,i;
	const unsigned char temp[4]={0xFF,0xFF,0x03,0x5A};
	
	a=0;
	
	Ds2Com(CMD_config_mode_enter,sizeof(CMD_config_mode_enter));
	for(i=0;i<200;i++);
	Ds2Com(CMD_query_DS2_analog_mode,sizeof(CMD_query_DS2_analog_mode));
	for(i=0;i<200;i++);
	for(i=0;i<4;i++){
		if(PADRD[i+3]!=temp[i])
			a++;
	}
 	if(i==0){
		Ds2Com(CMD_set_DS2_native_mode,sizeof(CMD_set_DS2_native_mode));
		for(i=0;i<200;i++);
		Ds2Com(CMD_config_mode_exit2,sizeof(CMD_config_mode_exit2));

		return 2;
	}

	else if(a==4){
		Ds2Com(CMD_set_mode_and_lock,sizeof(CMD_set_mode_and_lock));
		for(i=0;i<200;i++);
		Ds2Com(CMD_config_mode_exit,sizeof(CMD_config_mode_exit));
		return 1;
	}
	return 0;
}


void updatePAD()
{
	static unsigned char flag;
	int i;
	
	if(flag==2){
		Ds2Com(CMD_read_data,sizeof(CMD_read_data));	
		for(i=0;i<100;i++);
		Ds2Com(CMD_read_data,sizeof(CMD_read_data));	
	}
	else {
		Ds2Com(CMD_read_data2,sizeof(CMD_read_data2));
		for(i=0;i<100;i++);
		Ds2Com(CMD_read_data2,sizeof(CMD_read_data2));	
	}
	for(i=0;i<100;i++);

	switch(PADRD[2]){
	
		case 0x5a:
			if(PADRD[1]==0x73){
				PS_DATA[0]=~PADRD[3];
				PS_DATA[1]=~PADRD[4];
				for(i=2;i<6;i++)
					PS_DATA[i]=PADRD[3+i];
			}
			
			else if(PADRD[1]==0xF3){
				PS_DATA[0]=~PADRD[3];
				PS_DATA[1]=~PADRD[4];
				for(i=2;i<6;i++)
					PS_DATA[i]=PADRD[3+i];
			
			}
			 
			else{
				PS_DATA[0]=~PADRD[3];
				PS_DATA[1]=~PADRD[4];
				
				for(i=2;i<6;i++)
					PS_DATA[i]=0x80;
				flag=PS_Set_AN();
			}
			
		break;
		
		default:
			for(i=0;i<6;i++)
				PS_DATA[i]=0;
			for(i=2;i<6;i++)
				PS_DATA[i]=0x80;
		break;		

	}
}

BYTE getPAD(BYTE num){
	if(num <= PAD_AN_LY && num>= 0)
		return PS_DATA[num];
	else
		return 0;
}


BYTE MAINCYCLE;
//other ***********************************************************//
void Init(BYTE MainCycle)
{	
	MAINCYCLE = MainCycle;
	IENR1.BIT.IENWP = 0;
	
	//スイッチ入力p55 input & pull up
	IO.PMR5.BYTE= 0x03;	
	IO.PCR5=0x1C;
	IWPR.BYTE = 0x00;
	IO.PUCR5.BYTE=0xFF;
	
	//PS_CMD0,SEL3,CLK4
	IO.PCR2=0xFF;
	IO.PDR2.BIT.B3 = 1;
	IO.PDR2.BIT.B4 = 1;
	IO.PDR2.BIT.B0 = 1;
	
	//IO{EEP0124+BUZZER6+PS_DAT5}
	IO.PCR7=0xDF;
	
	IO.PDR7.BIT.B6=0;
	IO.PDR7.BIT.B4=1;
	
	//LCD+PS
	IO.PMR1.BYTE=0x00;
	IO.PCR1=0xFF;
	IO.PDR1.BYTE=0x00;
	
	//WD->off
	WDT.TCSRWD.BYTE=0x92;	
	WDT.TCSRWD.BYTE=0x92;
	
	WDT.TCSRWD.BIT.B4WI = 0;
	WDT.TCSRWD.BIT.TCSRWE=1;

	WDT.TCSRWD.BIT.B2WI=0;
	WDT.TCSRWD.BIT.WDON=0;
	
	WDT.TMWD.BYTE=0xFF;
	WDT.TCSRWD.BIT.B6WI = 1;
	WDT.TCSRWD.BIT.TCWE = 1;
	WDT.TCWD=0x00;
	
	//motor output logic
	IO.PCR3=0xFF;
	IO.PDR3.BYTE=0x00;
	
	//motor PWM & SV PWM
	IO.PCR6=0xFF;
	IO.PDR6.BYTE=0xFF;
	
	//TB1 maincycle
	if(MAINCYCLE < 30)
		MAINCYCLE = 30;
	TB1.TMB1.BYTE=0xF9;		//オートリロード、φ/2048
	TB1.TCB1 = (BYTE)(256.0-((14745600.0/2048.0)/(double)MAINCYCLE));
	
	IO.PCR8 = 0xFF;
	IO.PDR8.BYTE = 0x00;
	
	InitMtrPwm();
	InitTZ();
	InitMtrPwm();
	InitAD();
	InitSv();
}

BYTE getMainCycle(){
	return MAINCYCLE;
}

BYTE getSW()		//スイッチ読み込み
{
	BYTE x=0;

	if(IO.PDR5.BIT.B5==0)
		x+=1;
	return x;
}

//b0 LED1 ,b1 LED2
void LED(BYTE LedOn){
	IO.PDR6.BIT.B4 = ~LedOn&0x01;
	IO.PDR6.BIT.B5 = (~LedOn>>1)&0x01;
}

void Wait(int msec)
{
	double waittime,waitcnt = 0.0;
	waittime = 1000.0/(double)MAINCYCLE;
	do{
		Sync();
		waitcnt += waittime;
	}while(waitcnt < (double)msec);
}

UINT Sync()
{
	UINT aa = 0;
	WDT.TCWD=0x00;
	while(IRR2.BIT.IRRTB1!=1)
		aa++;
	IRR2.BIT.IRRTB1=0;
	return aa;
}
